//#include <iostream>
#include "CrankCamController.h"
#include <assert.h>

CrankCamController::CrankCamController(
		const Timer& _timer,
		const MaskBufType& toothPattern,
		AngleUnit _firstToothOffset,
		size_t missing_tooths)
:
	numTooth(toothPattern.size()),
	maxMissingTooth(missing_tooths),
	firstToothOffset(_firstToothOffset),
	timer(_timer),
	toothMask(toothPattern),
	toothMaskPos(toothMask.begin()),
	angleMask(numTooth - maxMissingTooth),
	angleMaskPos(angleMask.begin()),
	diffBuffer(numTooth - maxMissingTooth),
	diffBufferPos(diffBuffer.begin()),
	lastFixedAngle(0),
	lastFixedTime(0),
	revolutionPeriod(0),
	averageToothTime(0),
	isInSync(false)
{
    // it is required for firs bit in pattern to be 1
	assert(*toothMask.begin());

	// check that setup is correct: nuber of set bits == number of diffs
	unsigned short numBitsSet = 0;
	MaskBufType::const_iterator i;
	for (i = toothMask.begin(); i < toothMask.end(); i++) {
		if (*i) {
			numBitsSet++;
		}
	}
    assert(numBitsSet == diffBuffer.size());

    // TODO optional: fix possible rounding error when reminder is >0
    AngleUnit toothIncrement = 3600/numTooth;
    AngleBufType::iterator iangle = angleMask.begin();
	AngleUnit acc = 0;
	for (i = toothMask.begin(); i < toothMask.end(); i++) {
		acc += toothIncrement;
		if (*i) {
			*iangle = acc;
			acc = 0;
			iangle++;
		}
	}
	*(iangle-1) += acc;

//	printDiffBuffer(angleMask);

    reset();

}


//  TODO a bit alter: add second interrupt method for cam, be aware of 7200 revolution cycle

void CrankCamController::interruptToothEvent() {
	// remember time first of all
	TimeUnit newFixedTime = timer.getTime();
	TimeUnit diff = newFixedTime - lastFixedTime;
	lastFixedTime = newFixedTime;

	*diffBufferPos = diff;

	if (++diffBufferPos == diffBuffer.end())
		diffBufferPos = diffBuffer.begin();

	TimeBufType::iterator nextDiffBufferPos = diffBufferPos;
	if (nextDiffBufferPos == diffBuffer.end())
		nextDiffBufferPos = diffBuffer.begin();


	//if we dont have full info because next pos is not set yet
	// exit - next item should be filled
	if (!isInSync && *nextDiffBufferPos <=0) {
		return;
	}

	revolutionPeriod = 0;
	// TODO replace with rolling sum, maybe rest on full revolution
	TimeBufType::iterator j;
	for (j = diffBuffer.begin(); j < diffBuffer.end(); j++)
		revolutionPeriod += *j;
	averageToothTime = revolutionPeriod / numTooth;

	//check rythm - reset if something wrong
	// TODO creating mask object here is redundant, need just check
	if (!isInSync) {
		// we did not match tooth pattern yet

		if (!checkToothsPattern()) {
			return;
		}
		isInSync = true;
		// 1st tooth just arrived
		toothMaskPos = toothMask.begin();
		lastFixedAngle = firstToothOffset;
		angleMaskPos = angleMask.begin();
	} else {
		// we are in sync, just check that happened delay matched anticipated
		TimeUnit expectedDiff = 0;
		do {
			expectedDiff += averageToothTime;
			if (++toothMaskPos==toothMask.end())
				toothMaskPos=toothMask.begin();
		} while (!*toothMaskPos);

		if (!almostEqual(expectedDiff, diff)) {
			reset();
			return;
		}
		lastFixedAngle += *angleMaskPos;
		if (lastFixedAngle>=3600)
			lastFixedAngle -= 3600;

		if (++angleMaskPos==angleMask.end()) {
			// full turn
			lastFixedAngle = firstToothOffset;
			angleMaskPos=angleMask.begin();
		}

	}
	// all ok, dunno what else to do here

}

bool CrankCamController::checkToothsPattern() {
    TimeBufType::const_iterator diffBufferIterator = diffBuffer.begin();
    MaskBufType::const_iterator maskPosition = toothMask.begin();
	while (maskPosition < toothMask.end() && diffBufferIterator < diffBuffer.end()) {
		for (int i = 1; i <= maxMissingTooth+1; ++i) {
			if (almostEqual(averageToothTime * i, *diffBufferIterator)) {
				++diffBufferIterator;
				if (!*maskPosition)
					return false;
				++maskPosition;
				break;
			} else {
				if (*maskPosition)
					return false;
				++maskPosition;
			}
		}
	}
	return(maskPosition==toothMask.end() && diffBufferIterator == diffBuffer.end());
}

AngleUnit CrankCamController::predictAngleForTime(TimeUnit time) {
	if (revolutionPeriod==0)
		return -1;
	TimeUnit distance = time - lastFixedTime;
	while (distance > revolutionPeriod)
		distance -= revolutionPeriod;
	// TODO later: check that multiply does not cause overflow
	AngleUnit ret = lastFixedAngle + distance * 3600 / revolutionPeriod;
	if (ret >= 3600)
		ret -= 3600;
	return ret;
}

TimeUnit CrankCamController::predictTimeForAngle(AngleUnit angle) {
	AngleUnit distance = (angle < lastFixedAngle) ? angle+3600 - lastFixedAngle : angle - lastFixedAngle;
	return lastFixedTime + revolutionPeriod * distance / 3600;
}


AngleUnit CrankCamController::getCurrentAngle() {
	return lastFixedAngle;
}

void CrankCamController::reset() {
	isInSync = false;
	toothMaskPos = toothMask.begin();
	lastFixedAngle = 0;
	lastFixedTime = 0;

	revolutionPeriod = 0;
	averageToothTime = 0;

	TimeBufType::iterator j;
	for (j = diffBuffer.begin(); j < diffBuffer.end(); j++)
		*j = 0;
	diffBufferPos = diffBuffer.begin();

}

bool almostEqual(int n, int m) {
	return n >= 0 && m >= 0 && (n+(n>>2)) >= m && (m+(m>>2)) >= n;
}

/*
template <class TT> void CrankCamController::printDiffBuffer(vector<TT> buff) {
    cout << " [";
    for(unsigned i=0; i<buff.size(); i++) {
    	if (i>0)
            cout << ",";
        cout << buff[i];
    }
    cout << "]\n";
    cout.flush();
}
*/

CrankCamController::~CrankCamController () {
}


